#include <mc_perception/sensor_base.h>

#include <pcl_conversions/pcl_conversions.h> // For pcl::toROSMsg
#include <pcl/common/transforms.h> // For pcl::transformPointCloud
#include <pcl/common/common.h>  // For pcl::concatenatePointCloud

namespace msg_convert{

    // geometry_msgs::TransformStamped -> Eigen::Affine3f
    Eigen::Affine3f Geometry2Affine(geometry_msgs::TransformStamped transform){
        Eigen::Translation3f translation(transform.transform.translation.x,
            transform.transform.translation.y,
            transform.transform.translation.z);
        Eigen::Quaternionf rotation(transform.transform.rotation.w,
            transform.transform.rotation.x,
            transform.transform.rotation.y,
            transform.transform.rotation.z);     
        Eigen::Affine3f transform_matrix = translation * rotation;
        return transform_matrix;    
    }
    
   // 转换2D数据为点云数据 Scan2DPtr -> PointCloudXYZPtr
    PointCloudXYZPtr convertToPointCloudXYZ(
        Scan2DPtr scan_ptr){
        PointCloudXYZPtr pointcloud_ptr(new PointCloudXYZ);
        for(size_t i=0; i<scan_ptr->ranges.size(); ++i){
            float angle = scan_ptr->angle_min + i * scan_ptr->angle_increment;
            float range = scan_ptr->ranges[i];
            pcl::PointXYZ point;
            point.x = range * cos(angle);
            point.y = range * sin(angle);
            point.z = 0.0;
            pointcloud_ptr->points.emplace_back(point);
        }
        pointcloud_ptr->header.frame_id = scan_ptr->header.frame_id; // 设置坐标系
        // pointcloud_ptr->header.stamp = scan_ptr->header.stamp.toNSec();
        pointcloud_ptr->height = 1; // 单层点云
        pointcloud_ptr->width = pointcloud_ptr->points.size();  
        pointcloud_ptr->is_dense = false; 
        return pointcloud_ptr;
    }


    // 转换点云数据  PointCloud2Ptr -> PointCloudXYZPtr
    PointCloudXYZPtr convertToPointCloudXYZ(
        PointCloud2Ptr pointcloud2_ptr){
        PointCloudXYZPtr pcl_cloud_ptr(new PointCloudXYZ);
        pcl::fromROSMsg(*pointcloud2_ptr, *pcl_cloud_ptr);
        return pcl_cloud_ptr;
    }

    // 转换到PointCloud2  PointCloudXYZPtr -> sensor_msgs::PointCloud2
    sensor_msgs::PointCloud2Ptr convertToPointCloud2(PointCloudXYZPtr pointcloud_ptr){
        PointCloud2Ptr output_ptr = boost::make_shared<PointCloud2>();
        if(!pointcloud_ptr) return nullptr;
        pcl::toROSMsg(*pointcloud_ptr, *output_ptr);
        output_ptr->header.frame_id = pointcloud_ptr->header.frame_id;
        output_ptr->header.stamp = ros::Time(pointcloud_ptr->header.stamp);    
        return output_ptr;        
    }

    // TF变换 pcl::PointCloud<pcl::PointXYZ>::Ptr  Eigen::Affine3f
    void transformPointXYZ(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in,
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out, const Eigen::Affine3f &transform){
        // 使用 PCL 内置的 transformPointCloud 函数进行变换
        pcl::transformPointCloud(*cloud_in, *cloud_out, transform);
    }    

    // sensor_msgs::PointCloud2::Ptr -> sensor_msgs::PointCloud
    sensor_msgs::PointCloud convertToPointCloud(sensor_msgs::PointCloud2Ptr pointcloud2_ptr){
        sensor_msgs::PointCloud out_pointcloud;
        sensor_msgs::convertPointCloud2ToPointCloud(*pointcloud2_ptr, out_pointcloud);
        return out_pointcloud;
    }   
};  // namespace